#ifndef MODBUS_SLAVER_H
#define MODBUS_SLAVER_H
#include <vector>
#include <string>
#include <queue>
#include <mutex>
#include <thread>
#include <variant>
#include <chrono>
 
#include "modbus.h"
#include <std_msgs/msg/u_int8_multi_array.hpp>
#include <std_msgs/msg/u_int16_multi_array.hpp>
#include <std_msgs/msg/multi_array_dimension.hpp>
#include <agv_msgs/msg/mb_cmd_state.hpp>
#include <agv_msgs/msg/mb_slave_states.hpp>
#include "modbus_communication.h"

#define USEC_OF_1S 1000000
#define TOOFAST_SLEEP_SEC 1
#define Bits_PER_BYTE 8
#define NOW_TIME std::chrono::system_clock::now()


template <class T1>
class writeCFG
{
private:

    std::string name_;
public:
    writeCFG(std::string name)
    {
        name_ = name;
    };
    ~writeCFG(){};
    void writeValue_cb(const T1& mbInfo) {
        wCFG = mbInfo;
    }

    T1 readCFG() { return wCFG;}
public:
    T1 wCFG;   //coils
    T1 wCFG_last;   //coils
};

class ModbusSlave :public ModbusCommunication
{
public:
    enum ModbusPointType {
        COIL_STATUS = 0,
        INPUT_STATUS = 1,
        HOLDING_REGISTER =  2,
        INPUT_REGISTER =  3,
    };
    enum ElementValueType {
        BOOL_T = 0,
        INT16_T ,
        UINT16_T,
        Int32_T = 1,
        UInt32_T = 2,
        FLOAT_T =  3,
    };

    explicit ModbusSlave(const std::shared_ptr<rclcpp::Node> node);
    explicit ModbusSlave(const std::shared_ptr<rclcpp::Node> node, std::string name);
    ~ModbusSlave();
        /*!
    * \brief The client cycling read struct. 
    * Used for read modbus slave data .
    */
/*
    struct elementParamater {
        int address;
        int type;
        ros::Publisher pub;
        ros::Subscriber  sub,
        std::string topic_name;
    };
    typedef struct elementParamater ele_Para;
*/
    /*!
    * \brief The client cycling read struct. 
    * Used for read modbus slave data .
    */
   
    using SubVariant = std::variant<
        rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr,
        rclcpp::Subscription<std_msgs::msg::UInt16MultiArray>::SharedPtr
    >;

    struct CycleParamater {
        int index;
        int slaveID;
        int tickCount;        
        int tickexpire;
        int function;
        ModbusPointType arr_type;
        void* pMapping;
        int address;
        int number; 
        rclcpp::PublisherBase::SharedPtr pub;
        rclcpp::Publisher<agv_msgs::msg::MBSlaveStates>::SharedPtr statepub;
        SubVariant sub;
        std::string topic_name;
        std::string topic_name_sub;
        int elements_number; 
        std::shared_ptr<writeCFG<std_msgs::msg::UInt8MultiArray>> wCFG_bit = NULL;   //coils
        std::shared_ptr<writeCFG<std_msgs::msg::UInt16MultiArray>> wCFG_reg = NULL;  //reggisters
        std::chrono::system_clock::time_point begin_;
        std::chrono::system_clock::time_point end_;
    };
    typedef struct CycleParamater MB_cyclePara;

    struct slaveParamater {
        std::vector<MB_cyclePara> readParas; 
        std::string deviceName;
        int  perCount;
        int  cmdNum;
    };
    typedef struct slaveParamater Slave_Para;
    
    virtual void start() override ;
    virtual void stop() override ;
    virtual int init(std::string prefix) override;
    virtual int init(std::string host_ip,int host_port,int slave_id) override;
    int  Execute_Cmd(MB_cyclePara& cmdPara);
    void threadStatePub(float pollelapsed_secs, float interval_secs);
protected:
    void thread_cycle(ModbusSlave *pBase);
    int connect_host();
    bool reconnect_host();
    bool polling();
public:
    std::vector<Slave_Para> m_Slaves;  
private:
    float m_pollInterval;
    int m_threadInterval;
    rclcpp::Publisher<agv_msgs::msg::MBSlaveStates>::SharedPtr threadStatePub_;

    void pubMsg(MB_cyclePara& cmdPara);
    void copy2mapping(uint8_t *dest, int len, std_msgs::msg::UInt8MultiArray &newData);
    void copy2mapping(uint16_t *dest, int len, std_msgs::msg::UInt16MultiArray &newData);
    bool needPublish(int function){ return (function <= MODBUS_FC_READ_INPUT_REGISTERS);}
};

#endif
